/*
 * superPacketHandler.h
 *
 * Created: 9/19/2013 11:25:55 PM
 *  Author: Tim
 */ 


#ifndef SUPERPACKETHANDLER_H_
#define SUPERPACKETHANDLER_H_

#include "../structs/msgHeaderDefs.h"

#include "../handlers/genPacketHandler.h"
#include "../handlers/dioPacketHandler.h"SS
#include "../handlers/dacPacketHandler.h"


#include "../../features/allocations.h"

#include "../../features/dac/dac.h"
#include "../../features/dio/dio.h"
#include "../../features/enc/enc.h"
#include "../../features/tmr/tmr.h"

#define servo_MIN	600
#define servo_MAX	2400

static inline status_t outputSuperPacketHandler(volatile uint8_t packet[])
{
		WORD_t dacData;
		WORD_t digData;
		
		packet+=2;
	
		//ANALOG OUTPUT
		if(daqAlloc.analogAlloc.analogOutCh0Alloc == FEATURE_UTILIZED)
		{
			dacData.bytes.byteH = *packet++ & 0x0F;
			dacData.bytes.byteL = *packet++;
			
			dacUpdate(DAC_CH0_bp,dacData.word);
		}
		if(daqAlloc.analogAlloc.analogOutCh1Alloc == FEATURE_UTILIZED)
		{
			dacData.bytes.byteH = *packet++ & 0x0F;
			dacData.bytes.byteL = *packet++;
			
			dacUpdate(DAC_CH1_bp,dacData.word);
		}
		
		//DIGITAL OUTPUT
		if(daqAlloc.digitalOut == FEATURE_UTILIZED)
		{
			digData.bytes.byteH = *packet++ & 0x1F;
			digData.bytes.byteL = *packet++ & 0xFF;
			
			dioPortOutSet(digData.word);
			
			digData.bytes.byteH = *packet++ & 0x1F;
			digData.bytes.byteL = *packet++ & 0xFF;
			
			dioPortOutClr(digData.word);
		}
		
		//this is where a special ack code would be applicable
		//sendGenAck(outputSuperPacket_mh,1);
		return OPP_SUCCESSFUL;
		
}

static inline status_t actuatorSuperPacketHandler(uint8_t *packet)
{
	WORD_t actuatorData;	
	
	packet += 2;
		
	//SERVOS	
	if(daqAlloc.actuatorAlloc.servo0Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		
		if		(actuatorData.word < servo_MIN) actuatorData.word = servo_MIN;
		else if (actuatorData.word > servo_MAX) actuatorData.word = servo_MAX;
		
		TCE0.CCA = actuatorData.word>>1;
	}
	if(daqAlloc.actuatorAlloc.servo1Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		
		if		(actuatorData.word < servo_MIN) actuatorData.word = servo_MIN;
		else if (actuatorData.word > servo_MAX) actuatorData.word = servo_MAX;
		
		TCE0.CCB = actuatorData.word>>1;
	}
	if(daqAlloc.actuatorAlloc.servo2Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		
		if		(actuatorData.word < servo_MIN) actuatorData.word = servo_MIN;
		else if (actuatorData.word > servo_MAX) actuatorData.word = servo_MAX;
		
		TCE0.CCC = actuatorData.word>>1;
	}
	if(daqAlloc.actuatorAlloc.servo3Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		
		if		(actuatorData.word < servo_MIN) actuatorData.word = servo_MIN;
		else if (actuatorData.word > servo_MAX) actuatorData.word = servo_MAX;
		
		TCE0.CCD = actuatorData.word>>1;
	}
	
	///I think I need to swap & 0x04 for & 0x07
	
	//MOTORS
	if(daqAlloc.actuatorAlloc.motor0Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		if(actuatorData.word > 1024) actuatorData.word = 1024;
		
		TCD0.CCA = actuatorData.word;
	}
	if(daqAlloc.actuatorAlloc.motor1Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		if(actuatorData.word > 1024) actuatorData.word = 1024;
		
		TCD0.CCB = actuatorData.word;
	}
	if(daqAlloc.actuatorAlloc.motor2Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		if(actuatorData.word > 1024) actuatorData.word = 1024;
		
		TCD0.CCC = actuatorData.word;
	}
	if(daqAlloc.actuatorAlloc.motor3Alloc == FEATURE_UTILIZED)
	{
		actuatorData.bytes.byteH = *packet++;
		actuatorData.bytes.byteL = *packet++;
		if(actuatorData.word > 1024) actuatorData.word = 1024;
	
		TCD0.CCD = actuatorData.word;
	}
		
	//sendGenAck(actuatorSuperPacket_mh,1);
	return OPP_SUCCESSFUL;	
}

#endif /* SUPERPACKETHANDLER_H_ */